#!/usr/bin/env python3
# coding:utf-8
import rospy
import math
from get_path.srv import *
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from std_msgs.msg import Bool
from nav_msgs.msg import Path
from sensor_msgs.msg import NavSatFix
from geo import enu_to_geodetic, geodetic_to_enu
from coordTransform_utils import wgs84_to_gcj02, gcj02_to_wgs84


class PathClient:
    def __init__(self):
        # 参考点（WGS84）
        self.ref_point = [31.128376, 121.604065, 0.0]

        # 起点/终点（ENU）
        self.origin_enu = [float("inf"), float("inf")]
        self.destination_enu = [float("inf"), float("inf")]

        # 路径点 & 索引
        self.poses_planning = []
        self.current_index = 0

        # ROS
        self.pub_path = rospy.Publisher('/path_info', Path, queue_size=10)
        self.pub_ref = rospy.Publisher('/ref_point_wgs84', NavSatFix, queue_size=10)
        self.pub_nextgoal = rospy.Publisher("/next_goal", PoseStamped, queue_size=10)

        rospy.Subscriber("/goal_reached", Bool, self.goal_reached_callback)
        rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.get_destination)
        rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self.get_origin)

        # 服务
        rospy.loginfo("Waiting Service : /path_get_service")
        rospy.wait_for_service("/path_get_service")
        self.path_client = rospy.ServiceProxy("path_get_service", Pathget)
        rospy.loginfo("Ready Service : /path_get_service")

    def sample_path_by_distance(self, poses, dist_step=2.0):
        """对生成的路径点按距离采样"""
        def dist2d(p1, p2):
            dx = p1.pose.position.x - p2.pose.position.x
            dy = p1.pose.position.y - p2.pose.position.y
            return math.sqrt(dx*dx + dy*dy)

        if not poses:
            return []
        sampled = [poses[0]]
        accum = 0.0
        for i in range(1, len(poses)):
            accum += dist2d(poses[i], poses[i-1])
            if accum >= dist_step:
                sampled.append(poses[i])
                accum = 0.0
        if sampled[-1] != poses[-1]:
            sampled.append(poses[-1])  # 确保终点
        return sampled

    def get_path_points(self):
        """计算路径并发布第一个goal"""
        if self.ref_point[0] > 999999999:
            rospy.logwarn("Current reference point is invalid!")
            return

        # ENU -> WGS84
        dest_wgs84 = list(enu_to_geodetic(self.destination_enu[0], self.destination_enu[1], 0,
                                          self.ref_point[0], self.ref_point[1], self.ref_point[2]))
        origin_wgs84 = list(enu_to_geodetic(self.origin_enu[0], self.origin_enu[1], 0,
                                            self.ref_point[0], self.ref_point[1], self.ref_point[2]))

        # WGS84 -> GCJ02
        origin_gcj02 = wgs84_to_gcj02(origin_wgs84[0], origin_wgs84[1])
        dest_gcj02 = wgs84_to_gcj02(dest_wgs84[0], dest_wgs84[1])

        try:
            resp2 = self.path_client.call(origin_gcj02, dest_gcj02)
            response_path = resp2.pathfeedback
            rospy.loginfo("There are %d points in the path" % resp2.points_num)

            poses = []
            for i in response_path:
                pose_wgs84 = gcj02_to_wgs84(i.latitude, i.longitude)
                x1, y1, z1 = geodetic_to_enu(pose_wgs84[0], pose_wgs84[1], 0,
                                             self.ref_point[0], self.ref_point[1], self.ref_point[2])
                pose = PoseStamped()
                pose.header.frame_id = "map"
                pose.pose.position.x = x1
                pose.pose.position.y = y1
                pose.pose.position.z = 0
                pose.pose.orientation.w = 1.0
                poses.append(pose)

            # 采样
            self.poses_planning = self.sample_path_by_distance(poses, dist_step=2.0)

            # 发布第一个点
            if self.poses_planning:
                self.current_index = 0
                first_goal = self.poses_planning[self.current_index]
                first_goal.header.stamp = rospy.Time.now()
                self.pub_nextgoal.publish(first_goal)
                rospy.loginfo("Published first goal at (%.2f, %.2f)",
                              first_goal.pose.position.x,
                              first_goal.pose.position.y)

            # 发布整条路径（调试用）
            arrGps_planning = Path()
            arrGps_planning.header.stamp = rospy.Time.now()
            arrGps_planning.header.frame_id = "map"
            arrGps_planning.poses = self.poses_planning
            self.pub_path.publish(arrGps_planning)

        except rospy.ServiceException as e:
            rospy.logwarn("path_get_service Service call failed: %s" % e)

    def get_destination(self, destination):
        """根据rviz的2D Nav Goal 得到的终点（ENU表示）"""
        self.destination_enu = [destination.pose.position.x, destination.pose.position.y]
        if self.origin_enu[0] < 999999999:
            self.get_path_points()
        else:
            rospy.logwarn("Current origin point is invalid!")

    def get_origin(self, origin):
        self.origin_enu = [origin.pose.pose.position.x, origin.pose.pose.position.y]
        if self.destination_enu[0] < 999999999:
            self.get_path_points()
        else:
            rospy.logwarn("Current destination point is invalid!")

    def goal_reached_callback(self, msg):
        if not self.poses_planning:
            rospy.logwarn("No planned poses available.")
            return
        if msg.data:  # True 表示到达当前点
            if self.current_index < len(self.poses_planning) - 1:
                self.current_index += 1
                next_goal = self.poses_planning[self.current_index]
                next_goal.header.stamp = rospy.Time.now()
                self.pub_nextgoal.publish(next_goal)
                rospy.loginfo("Published next goal index %d: (%.2f, %.2f)",
                              self.current_index,
                              next_goal.pose.position.x,
                              next_goal.pose.position.y)
            else:
                rospy.loginfo("All goals reached ✅")

    def spin(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            # 持续发布 ref_point 供调试用
            msg = NavSatFix()
            msg.latitude = self.ref_point[0]
            msg.longitude = self.ref_point[1]
            msg.altitude = self.ref_point[2]
            self.pub_ref.publish(msg)
            rate.sleep()


if __name__ == "__main__":
    rospy.init_node('path_client', anonymous=True)
    client = PathClient()
    client.spin()
    rospy.spin()
